#!/usr/bin/env roseus
;; -*- mode: EusLisp; coding: utf-8-unix; -*-
#-:jsk
(jsk)
#-:rbrain-basic
(rbrain)

(load (format nil "~A/euslib/demo/h-yaguchi/hrp2vk/hand-functions.l"
              (unix::getenv "CVSDIR")))

(ros::roseus-add-msgs "roseus")

(setq *open-av* (float-vector -65.0 -88.0 0.0 -40.0 0))

(defun prepare ()
  (unless (boundp '*hrp2*) (hrp2vk-simple))
  (unless (boundp '*pickview*) (pickview))
  (objects (list *hrp2*)))

;; (prepare)

(defun prepare-real ()
  (load (format nil "~A/hrp2/corba/orbixif.l" (unix::getenv "CVSDIR")))
  (load (format nil "~A/hrp2/plugins/plugins.l" (unix::getenv "CVSDIR")))
  (unless (boundp '*ci*) (eusorbix-init))
  (connect-hands)
  (send *ns* :controlmode-vector
	(concatenate float-vector
		     (make-array 5 :element-type float-vector :initial-element 0)
		     (make-array 5 :element-type float-vector :initial-element 8))))
(warn "(prepare-real)~%")

(defun grasp-present (&key (real nil) (debug nil) )
  ;; prepare hand
  (send *hrp2* :larm :hand :angle-vector *open-av*)
  (send *pickview* :draw-objects)
  (when debug (read-line))
  (if real (send-angle-vector-hands))
  ;;
  (let ((av-lst
         (list 
          (float-vector 19.3206 59.8536 -37.7516 -59.211 6.3168 -26.589 -85.1144 -15.0) ;;pre motion
          (float-vector 19.6222 51.2565 -38.8339 -82.865 6.77353 -21.6091 -56.7527 -15.0) ;;grasp motion
          (float-vector 19.3206 59.8536 -37.7516 -59.211 6.3168 -26.589 -85.1144 -15.0) ;;post motion
          (float-vector -32.1858 25.7893 -26.9749 -52.4182 29.7107 -25.5043 -14.0853 -60.0) ;; present motion
         ))
        (cntr 0))
    (send *hrp2* :reset-manip-pose)
    (send *pickview* :draw-objects)
    (when debug (read-line))
    (when real
      (send *ci* :angle-vector (send *hrp2* :angle-vector) 10000 "wor")
      (send *ci* :wait-interpolation "wor"))
    (dolist (l av-lst)
      (send *hrp2* :larm :angle-vector l)
      (send *pickview* :draw-objects)
      (when debug (read-line))
      (when real
        (send *ci* :angle-vector (send *hrp2* :angle-vector) 10000 "wor")
        (send *ci* :wait-interpolation "wor"))
      (when (= cntr 1)
        (send *hrp2* :larm :hand :thick-pinch)
        (send *pickview* :draw-objects)
        (when debug (read-line))
        (if real (send-angle-vector-hands)))
      (incf cntr))
    (send *hrp2* :head :look-at
	  (send (send (send *hrp2* :larm :end-coords :copy-worldcoords)
		      :translate (float-vector 100 0 0)) :worldpos))
    (when nil;real
      ;; prepare ros
      (setq *buf* nil)
      (ros::subscribe "state_result" roseus::StringStamped #'get-result 1)
      (while (not *buf*)
	(ros::sleep)
        (ros::spin-once))
      (ros::unsubscribe "state_result"))
      ;;
    (send *hrp2* :larm :hand :angle-vector *open-av*)
    (send *pickview* :draw-objects)
    (when debug (read-line))
    (if real (send-angle-vector-hands))
    (send *hrp2* :reset-manip-pose)
    (send *pickview* :draw-objects)
    (when debug (read-line))
    (when real
      (send *ci* :angle-vector (send *hrp2* :angle-vector) 10000 "wor")
      (send *ci* :wait-interpolation "wor"))
    ))

(defun get-result (msg)
  (when (string= (send msg :data) "both")
    (setq *buf* t)))